#! /usr/bin/env python
"""
    需求: 实现基本的话题通信，一方发布数据，一方接收数据
"""
#1.导包 
import rospy
import sys
from time import sleep
sys.path.append("/home/galm/ms_01/src/command_receiver/scripts/query_controller")
sys.path.append("/home/galm/ms_01/src/command_handler/scripts")
from command_receiver.msg import roboCmd
from commandSubcriber import CommandSubscriber

def doMsg(msg):
    rospy.loginfo("I heard:%s",msg)
    subscriber.cmdParse(msg)
    

if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node(rospy.get_param("HandlerName","command_handler"))
    #3.实例化 订阅者 对象
    sub = rospy.Subscriber(rospy.get_param("QCTopic","user_cmd"),roboCmd,doMsg,queue_size=10)
    rospy.loginfo("loading...")
    sleep(3)
    #4.处理订阅的消息(回调函数)
    subscriber = CommandSubscriber()
    #5.设置循环调用回调函数
    rospy.spin()
